#include "ParticleFilter.h"

#include "Observation.h"
#include "RandomDistribution.h"

#include "math.h"
#include "omp.h"


ParticleFilter::ParticleFilter() :
    QObject(),
    randomDistribution(new RandomDistribution())
{

}

ParticleFilter::~ParticleFilter()
{
    delete randomDistribution;
}

void ParticleFilter::reset()
{
    bagOfSamples.clear();
}


void ParticleFilter::start(QVector<Observation>& observations, int numberOfSamples)
{    
    this->reset();

    foreach (Observation observation, observations) {
        this->performLikelihoodWeighting(observation, numberOfSamples);

        double meanX = 0.0;
        double meanY = 0.0;
        double varianceX = 0.0;
        double varianceY = 0.0;

        double totalLikelihood = 0.0;
        foreach (Sample sample, bagOfSamples) {
            totalLikelihood += observation.getLikelihood(sample.x(), sample.y());
        }

        foreach(Sample sample, bagOfSamples)
        {
            double p = observation.getLikelihood(sample.x(), sample.y()) / totalLikelihood;
            meanX += p * sample.x();
            meanY += p * sample.y();
        }
        foreach(Sample sample, bagOfSamples)
        {
            double p = observation.getLikelihood(sample.x(), sample.y()) / totalLikelihood;
            varianceX += p * pow(sample.x() - meanX, 2.0);
            varianceY += p * pow(sample.y() - meanY, 2.0);
        }

        emit resultReady(meanX, meanY, varianceX, varianceY);
    }
}

void ParticleFilter::performLikelihoodWeighting(Observation& observation, int numberOfSamples)
{
    if (bagOfSamples.isEmpty()) {
        // Fill up with priors
        // sample from prior distribution (x_0, y_0)
        for (int i = 0; i < numberOfSamples; i++) {
            double x = randomDistribution->getRandomNormal(0, 1);
            double y = randomDistribution->getRandomNormal(0, 1);
            bagOfSamples.append(Sample(x, y));
        }
    }

    this->performSystemTransition();


    QVector<double> likelihoods;
    foreach (Sample sample, bagOfSamples) {
        double likelihood = observation.getLikelihood(sample.x(), sample.y());

        likelihoods.append(likelihood);
    }

    this->resample(likelihoods);
}

void ParticleFilter::performSystemTransition()
{
    QVector<Sample> newBagOfSamples;
    foreach (Sample sample, bagOfSamples) {
        double d = randomDistribution->getRandomNormal(5, 1);
        double theta = randomDistribution->getRandomUniform(M_PI/5 - M_PI/36, M_PI/5 + M_PI/36);

        Sample newSample(sample.x() + d * cos(theta), sample.y() + d * sin(theta));
        newBagOfSamples.append(newSample);

    }

    bagOfSamples = newBagOfSamples;
}

void ParticleFilter::resample(QVector<double>& likelihoods)
{    
    double totalLikelihood = 0;    
    for (int i = 0; i < bagOfSamples.count(); i++) {
        totalLikelihood += likelihoods[i];
    }

    QVector<Sample> newBagOfSamples;

    for (int i = 0; i < bagOfSamples.count(); i++) {
        double random = randomDistribution->getRandomUniform(0, totalLikelihood);

        double cumulative = 0.0;
        for (int j = 0; j < bagOfSamples.count(); j++) {

            if (random >= cumulative && random < cumulative + likelihoods[j]) {
                newBagOfSamples.append(bagOfSamples[j]);
                break;
            }

            cumulative += likelihoods[j];
        }
    }

    bagOfSamples = newBagOfSamples;
}
